using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 使用旋转铰链的角度组件约束两个实体,该旋转铰链允许绕两个轴旋转,类似于允许翻转屏幕的笔记本显示器铰链。
    /// </summary>
    public struct AngularSwivelHinge : ITwoBodyConstraintDescription<AngularSwivelHinge>
    {
        /// <summary>
        /// 身体A的局部空间内的旋转轴。
        /// </summary>
        public Vector3 LocalSwivelAxisA;
        /// <summary>
        /// 身体B局部空间的铰链轴线。
        /// </summary>
        public Vector3 LocalHingeAxisB;
        /// <summary>
        /// 弹簧频率和阻尼参数。
        /// </summary>
        public SpringSettings SpringSettings;

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return AngularSwivelHingeTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(AngularSwivelHingeTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            ConstraintChecker.AssertUnitLength(LocalSwivelAxisA, nameof(AngularSwivelHinge), nameof(LocalSwivelAxisA));
            ConstraintChecker.AssertUnitLength(LocalHingeAxisB, nameof(AngularSwivelHinge), nameof(LocalHingeAxisB));
            ConstraintChecker.AssertValid(SpringSettings, nameof(AngularSwivelHinge));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<AngularSwivelHingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.WriteFirst(LocalSwivelAxisA, ref target.LocalSwivelAxisA);
            Vector3Wide.WriteFirst(LocalHingeAxisB, ref target.LocalHingeAxisB);
            GetFirst(ref target.SpringSettings.AngularFrequency) = SpringSettings.AngularFrequency;
            GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio;
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AngularSwivelHinge description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<AngularSwivelHingePrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.ReadFirst(source.LocalSwivelAxisA, out description.LocalSwivelAxisA);
            Vector3Wide.ReadFirst(source.LocalHingeAxisB, out description.LocalHingeAxisB);
            description.SpringSettings.AngularFrequency = GetFirst(ref source.SpringSettings.AngularFrequency);
            description.SpringSettings.TwiceDampingRatio = GetFirst(ref source.SpringSettings.TwiceDampingRatio);
        }
    }

    public struct AngularSwivelHingePrestepData
    {
        public Vector3Wide LocalSwivelAxisA;
        public Vector3Wide LocalHingeAxisB;
        public SpringSettingsWide SpringSettings;
    }

    public struct AngularSwivelHingeProjection
    {
        // JacobianB=-JacobianA,所以不需要显式存储它。
        public Vector3Wide VelocityToImpulseA;
        public Vector<float> BiasImpulse;
        public Vector<float> SoftnessImpulseScale;
        public Vector3Wide ImpulseToVelocityA;
        public Vector3Wide NegatedImpulseToVelocityB;
    }

    public struct AngularSwivelHingeFunctions : IConstraintFunctions<AngularSwivelHingePrestepData, AngularSwivelHingeProjection, Vector<float>>
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref AngularSwivelHingePrestepData prestep, out AngularSwivelHingeProjection projection)
        {
            bodies.GatherOrientation(ref bodyReferences, count, out var orientationA, out var orientationB);

            // 旋转铰链试图使车身A上的轴与车身B上的轴分隔开90度。换句话说,这与铰链关节相同,但自由度少了一个。
            // C=圆点(旋转A,铰链B)=0
            // C‘=点(d/dt(旋转A),铰链B)+点(旋转A,d/dt(铰链B))=0
            // C‘=点(角度速度B x铰链B,旋转A)+点(铰链B,角度速度A x旋转A)=0
            // C‘=点(铰链B x旋转A,角度速度B)+点(角度速度A,旋转A x铰链B)=0
            // 提供以下方面的雅克比：
            // JA=旋转A x铰链B
            // JB=铰链B x旋转A
            // a x b==-b x a,所以jb==。

            // 现在,我们选择存储表示法。默认方法是存储JA、有效质量和两个逆惯性,需要6+1+6+6标量。
            // 另一种方法是存储JAT*EffectiveMasses,然后存储JA*inverseInertia张量(A/B),这只需要3+3+3个标量。
            // 因此,总的来说,预焙为我们节省了10个标量和一些迭代时间的ALU。
            QuaternionWide.TransformWithoutOverlap(prestep.LocalSwivelAxisA, orientationA, out var swivelAxis);
            QuaternionWide.TransformWithoutOverlap(prestep.LocalHingeAxisB, orientationB, out var hingeAxis);
            Vector3Wide.CrossWithoutOverlap(swivelAxis, hingeAxis, out var jacobianA);
            // 在轴平行的情况下,不存在唯一的雅可比。随便挑一个。
            // 请注意,这会导致两极的雅可比长度不连续。我们只是不担心这件事。
            Helpers.FindPerpendicular(swivelAxis, out var fallbackJacobian);
            Vector3Wide.Dot(jacobianA, jacobianA, out var jacobianLengthSquared);
            var useFallback = Vector.LessThan(jacobianLengthSquared, new Vector<float>(1e-7f));
            Vector3Wide.ConditionalSelect(useFallback, fallbackJacobian, jacobianA, out jacobianA);

            // 注意,JA=-JB,但为了计算有效质量,符号是无关的。

            // 这使用通常的(J*M^-1*JT)^-1公式来计算有效质量,但是我们实际上利用了中间结果J*M^-1,所以我们直接计算它。
            Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaA.InverseInertiaTensor, out projection.ImpulseToVelocityA);
            // 注意,我们在这里没有使用-JacobianA,所以我们实际上存储了转换的否定版本。这很好;我们只需在迭代中减去即可。
            Symmetric3x3Wide.TransformWithoutOverlap(jacobianA, inertiaB.InverseInertiaTensor, out projection.NegatedImpulseToVelocityB);
            Vector3Wide.Dot(projection.ImpulseToVelocityA, jacobianA, out var angularA);
            Vector3Wide.Dot(projection.NegatedImpulseToVelocityB, jacobianA, out var angularB);

            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
            var effectiveMass = effectiveMassCFMScale / (angularA + angularB);
            Vector3Wide.Scale(jacobianA, effectiveMass, out projection.VelocityToImpulseA);

            Vector3Wide.Dot(hingeAxis, swivelAxis, out var error);
            // 注意否定：我们想要反对分离。TODO：可以说,应该将否定烘焙到PositionErrorToVelocity(给定其名称)。
            projection.BiasImpulse = -effectiveMass * positionErrorToVelocity * error;

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ApplyImpulse(ref Vector3Wide angularVelocityA, ref Vector3Wide angularVelocityB, ref AngularSwivelHingeProjection projection, ref Vector<float> csi)
        {
            Vector3Wide.Scale(projection.ImpulseToVelocityA, csi, out var velocityChangeA);
            Vector3Wide.Add(angularVelocityA, velocityChangeA, out angularVelocityA);
            Vector3Wide.Scale(projection.NegatedImpulseToVelocityB, csi, out var negatedVelocityChangeB);
            Vector3Wide.Subtract(angularVelocityB, negatedVelocityChangeB, out angularVelocityB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularSwivelHingeProjection projection, ref Vector<float> accumulatedImpulse)
        {
            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref accumulatedImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref AngularSwivelHingeProjection projection, ref Vector<float> accumulatedImpulse)
        {
            // JB=-JA。这是(angularVelocityA*JA+angularVelocityB*JB)*EffectiveMass=>(angularVelocityA-angularVelocityB)*(JA*EffectiveMass)
            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var difference);
            Vector3Wide.Dot(difference, projection.VelocityToImpulseA, out var csi);
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csi;

            accumulatedImpulse += csi;
            ApplyImpulse(ref velocityA.Angular, ref velocityB.Angular, ref projection, ref csi);
        }

    }

    public class AngularSwivelHingeTypeProcessor : TwoBodyTypeProcessor<AngularSwivelHingePrestepData, AngularSwivelHingeProjection, Vector<float>, AngularSwivelHingeFunctions>
    {
        public const int BatchTypeId = 24;
    }
}

